/*
This file is part of CanFestival, a library implementing CanOpen Stack.

Copyright (C): Edouard TISSERANT and Francis DUPIN

See COPYING file for copyrights details.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#if defined(WIN32) && !defined(__CYGWIN__)
    #include <windows.h>
    #define CLEARSCREEN "cls"
    #define SLEEP(time) Sleep(time * 1000)
#else
    #include <unistd.h>
    #include <stdio.h>
    #include <string.h>
    #include <stdlib.h>
    #define CLEARSCREEN "clear"
    #define SLEEP(time) sleep(time)
#endif

//****************************************************************************
// INCLUDES
#include "canfestival.h"
#include "CANOpenShell.h"
#include "CANOpenShellMasterOD.h"
#include "CANOpenShellSlaveOD.h"

//****************************************************************************
// DEFINES
#define MAX_NODES 127
#define cst_str4(c1, c2, c3, c4) ((((unsigned int)0 | \
                                    (char)c4 << 8) | \
                                   (char)c3) << 8 | \
                                  (char)c2) << 8 | \
                                            (char)c1

#define INIT_ERR 2
#define QUIT 1

//****************************************************************************
// GLOBALS
char BoardBusName[31];
char BoardBaudRate[5];
s_BOARD Board = {BoardBusName, BoardBaudRate};
CO_Data* CANOpenShellOD_Data;
char LibraryPath[512];

/* Sleep for n seconds */
void SleepFunction(int second)
{
#ifdef USE_RTAI
    sleep(second);
#else
    SLEEP(second);
#endif
}

/* Ask a slave node to go in operational mode */
void StartNode(UNS8 nodeid)
{
    masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Start_Node);
}

/* Ask a slave node to go in pre-operational mode */
void StopNode(UNS8 nodeid)
{
    masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Stop_Node);
}

/* Ask a slave node to reset */
void ResetNode(UNS8 nodeid)
{
    masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Reset_Node);
}

/* Reset all nodes on the network and print message when boot-up*/
void DiscoverNodes()
{
    printf("Wait for Slave nodes bootup...\n\n");
    ResetNode(0x00);
}

int get_info_step = 0;
/* Callback function that check the read SDO demand */
void CheckReadInfoSDO(CO_Data* d, UNS8 nodeid)
{
    UNS32 abortCode;
    UNS32 data = 0;
    UNS32 size = 64;
    if (getReadResultNetworkDict(CANOpenShellOD_Data, nodeid, &data, &size, &abortCode) != SDO_FINISHED)
    {
        printf("Master : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
    }
    else
    {
        /* Display data received */
        switch (get_info_step)
        {
        case 1:
            printf("Device type     : %x\n", data);
            break;
        case 2:
            printf("Vendor ID       : %x\n", data);
            break;
        case 3:
            printf("Product Code    : %x\n", data);
            break;
        case 4:
            printf("Revision Number : %x\n", data);
            break;
        }
    }
    /* Finalize last SDO transfer with this node */
    closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);
    GetSlaveNodeInfo(nodeid);
}

/* Retrieve node informations located at index 0x1000 (Device Type) and 0x1018 (Identity) */
void GetSlaveNodeInfo(UNS8 nodeid)
{
    switch (++get_info_step)
    {
    case 1: /* Get device type */
        printf("##################################\n");
        printf("#### Informations for node %x ####\n", nodeid);
        printf("##################################\n");
        readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1000, 0x00, 0, CheckReadInfoSDO, 0);
        break;
    case 2: /* Get Vendor ID */
        readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x01, 0, CheckReadInfoSDO, 0);
        break;
    case 3: /* Get Product Code */
        readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x02, 0, CheckReadInfoSDO, 0);
        break;
    case 4: /* Get Revision Number */
        readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x03, 0, CheckReadInfoSDO, 0);
        break;
    case 5: /* Print node info */
        get_info_step = 0;
    }
}

/* Callback function that check the read SDO demand */
void CheckReadSDO(CO_Data* d, UNS8 nodeid)
{
    UNS32 abortCode;
    UNS32 data = 0;
    UNS32 size = 64;
    if (getReadResultNetworkDict(CANOpenShellOD_Data, nodeid, &data, &size, &abortCode) != SDO_FINISHED)
    {
        printf("\nResult : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
    }
    else
    {
        printf("\nResult : %x\n", data);
    }
    /* Finalize last SDO transfer with this node */
    closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);
}

/* Read a slave node object dictionary entry */
void ReadDeviceEntry(char* sdo)
{
    int ret = 0;
    int nodeid;
    int index;
    int subindex;
    int datatype = 0;
    ret = sscanf(sdo, "rsdo#%2x,%4x,%2x", &nodeid, &index, &subindex);
    if (ret == 3)
    {
        printf("##################################\n");
        printf("#### Read SDO                 ####\n");
        printf("##################################\n");
        printf("NodeId   : %2.2x\n", nodeid);
        printf("Index    : %4.4x\n", index);
        printf("SubIndex : %2.2x\n", subindex);
        readNetworkDictCallback(CANOpenShellOD_Data, (UNS8)nodeid, (UNS16)index, (UNS8)subindex, (UNS8)datatype, CheckReadSDO, 0);
    }
    else
    {
        printf("Wrong command  : %s\n", sdo);
    }
}

/* Callback function that check the write SDO demand */
void CheckWriteSDO(CO_Data* d, UNS8 nodeid)
{
    UNS32 abortCode;
    if (getWriteResultNetworkDict(CANOpenShellOD_Data, nodeid, &abortCode) != SDO_FINISHED)
    {
        printf("\nResult : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
    }
    else
    {
        printf("\nSend data OK\n");
    }
    /* Finalize last SDO transfer with this node */
    closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);
}

/* Write a slave node object dictionnary entry */
void WriteDeviceEntry(char* sdo)
{
    int ret = 0;
    int nodeid;
    int index;
    int subindex;
    int size;
    int data;
    ret = sscanf(sdo, "wsdo#%2x,%4x,%2x,%2x,%x", &nodeid, &index, &subindex, &size, &data);
    if (ret == 5)
    {
        printf("##################################\n");
        printf("#### Write SDO                ####\n");
        printf("##################################\n");
        printf("NodeId   : %2.2x\n", nodeid);
        printf("Index    : %4.4x\n", index);
        printf("SubIndex : %2.2x\n", subindex);
        printf("Size     : %2.2x\n", size);
        printf("Data     : %x\n", data);
        writeNetworkDictCallBack(CANOpenShellOD_Data, nodeid, index, subindex, size, 0, &data, CheckWriteSDO, 0);
    }
    else
    {
        printf("Wrong command  : %s\n", sdo);
    }
}

void CANOpenShellOD_post_SlaveBootup(CO_Data* d, UNS8 nodeid)
{
    printf("Slave %x boot up\n", nodeid);
}

/***************************  CALLBACK FUNCTIONS  *****************************************/
void CANOpenShellOD_initialisation(CO_Data* d)
{
    printf("Node_initialisation\n");
}

void CANOpenShellOD_preOperational(CO_Data* d)
{
    printf("Node_preOperational\n");
}

void CANOpenShellOD_operational(CO_Data* d)
{
    printf("Node_operational\n");
}

void CANOpenShellOD_stopped(CO_Data* d)
{
    printf("Node_stopped\n");
}

void CANOpenShellOD_post_sync(CO_Data* d)
{
    //printf("Master_post_sync\n");
}

void CANOpenShellOD_post_TPDO(CO_Data* d)
{
    //printf("Master_post_TPDO\n");
}

/***************************  INITIALISATION  **********************************/
void Init(CO_Data* d, UNS32 id)
{
    if (Board.baudrate)
    {
        /* Init node state*/
        setState(CANOpenShellOD_Data, Initialisation);
    }
}

/***************************  CLEANUP  *****************************************/
void Exit(CO_Data* d, UNS32 nodeid)
{
    if (strcmp(Board.baudrate, "none"))
    {
        /* Reset all nodes on the network */
        masterSendNMTstateChange(CANOpenShellOD_Data, 0, NMT_Reset_Node);
        /* Stop master */
        setState(CANOpenShellOD_Data, Stopped);
    }
}

int NodeInit(int NodeID, int NodeType)
{
    if (NodeType)
    {
        CANOpenShellOD_Data = &CANOpenShellMasterOD_Data;
    }
    else
    {
        CANOpenShellOD_Data = &CANOpenShellSlaveOD_Data;
    }
#ifndef NOT_USE_DYNAMIC_LOADING
    /* Load can library */
    LoadCanDriver(LibraryPath);
#endif
    /* Define callback functions */
    CANOpenShellOD_Data->initialisation = CANOpenShellOD_initialisation;
    CANOpenShellOD_Data->preOperational = CANOpenShellOD_preOperational;
    CANOpenShellOD_Data->operational = CANOpenShellOD_operational;
    CANOpenShellOD_Data->stopped = CANOpenShellOD_stopped;
    CANOpenShellOD_Data->post_sync = CANOpenShellOD_post_sync;
    CANOpenShellOD_Data->post_TPDO = CANOpenShellOD_post_TPDO;
    CANOpenShellOD_Data->post_SlaveBootup = CANOpenShellOD_post_SlaveBootup;
    /* Open the Peak CANOpen device */
    if (!canOpen(&Board, CANOpenShellOD_Data))
    {
        return INIT_ERR;
    }
    /* Defining the node Id */
    setNodeId(CANOpenShellOD_Data, NodeID);
    /* Start Timer thread */
    StartTimerLoop(&Init);
    return 0;
}

void help_menu(void)
{
    printf("   MANDATORY COMMAND (must be the first command):\n");
    printf("     load#CanLibraryPath,channel,baudrate,nodeid,type (0:slave, 1:master)\n");
    printf("\n");
    printf("   NETWORK: (if nodeid=0x00 : broadcast)\n");
    printf("     ssta#nodeid : Start a node\n");
    printf("     ssto#nodeid : Stop a node\n");
    printf("     srst#nodeid : Reset a node\n");
    printf("     scan : Reset all nodes and print message when bootup\n");
    printf("     wait#seconds : Sleep for n seconds\n");
    printf("\n");
    printf("   SDO: (size in bytes)\n");
    printf("     info#nodeid\n");
    printf("     rsdo#nodeid,index,subindex : read sdo\n");
    printf("        ex : rsdo#42,1018,01\n");
    printf("     wsdo#nodeid,index,subindex,size,data : write sdo\n");
    printf("        ex : wsdo#42,6200,01,01,FF\n");
    printf("\n");
    printf("   Note: All numbers are hex\n");
    printf("\n");
    printf("     help : Display this menu\n");
    printf("     quit : Quit application\n");
    printf("\n");
    printf("\n");
}

int ExtractNodeId(char* command)
{
    int nodeid;
    sscanf(command, "%2x", &nodeid);
    return nodeid;
}

int ProcessCommand(char* command)
{
    int ret = 0;
    int sec = 0;
    int NodeID;
    int NodeType;
    EnterMutex();
    switch (cst_str4(command[0], command[1], command[2], command[3]))
    {
    case cst_str4('h', 'e', 'l', 'p') : /* Display Help*/
        help_menu();
        break;
    case cst_str4('s', 's', 't', 'a') : /* Slave Start*/
        StartNode(ExtractNodeId(command + 5));
        break;
    case cst_str4('s', 's', 't', 'o') : /* Slave Stop */
        StopNode(ExtractNodeId(command + 5));
        break;
    case cst_str4('s', 'r', 's', 't') : /* Slave Reset */
        ResetNode(ExtractNodeId(command + 5));
        break;
    case cst_str4('i', 'n', 'f', 'o') : /* Retrieve node informations */
        GetSlaveNodeInfo(ExtractNodeId(command + 5));
        break;
    case cst_str4('r', 's', 'd', 'o') : /* Read device entry */
        ReadDeviceEntry(command);
        break;
    case cst_str4('w', 's', 'd', 'o') : /* Write device entry */
        WriteDeviceEntry(command);
        break;
    case cst_str4('s', 'c', 'a', 'n') : /* Display master node state */
        DiscoverNodes();
        break;
    case cst_str4('w', 'a', 'i', 't') : /* Display master node state */
        ret = sscanf(command, "wait#%d", &sec);
        if (ret == 1)
        {
            LeaveMutex();
            SleepFunction(sec);
            return 0;
        }
        break;
    case cst_str4('q', 'u', 'i', 't') : /* Quit application */
        LeaveMutex();
        return QUIT;
    case cst_str4('l', 'o', 'a', 'd') : /* Library Interface*/
        ret = sscanf(command, "load#%100[^,],%30[^,],%4[^,],%d,%d",
                     LibraryPath,
                     BoardBusName,
                     BoardBaudRate,
                     &NodeID,
                     &NodeType);
        if (ret == 5)
        {
            LeaveMutex();
            ret = NodeInit(NodeID, NodeType);
            return ret;
        }
        else
        {
            printf("Invalid load parameters\n");
        }
        break;
    default :
        help_menu();
    }
    LeaveMutex();
    return 0;
}

/****************************************************************************/
/***************************  MAIN  *****************************************/
/****************************************************************************/

int main(int argc, char** argv)
{
    extern char* optarg;
    char command[200];
    char* res;
    int ret = 0;
    int sysret = 0;
    int i = 0;
    /* Print help and exit immediatly*/
    if (argc < 2)
    {
        help_menu();
        exit(1);
    }
    /* Init stack timer */
    TimerInit();
    /* Strip command-line*/
    for (i = 1 ; i < argc ; i++)
    {
        if (ProcessCommand(argv[i]) == INIT_ERR)
        {
            goto init_fail;
        }
    }
    /* Enter in a loop to read stdin command until "quit" is called */
    while (ret != QUIT)
    {
        // wait on stdin for string command
        res = fgets(command, sizeof(command), stdin);
        sysret = system(CLEARSCREEN);
        ret = ProcessCommand(command);
        fflush(stdout);
    }
    printf("Finishing.\n");
    // Stop timer thread
    StopTimerLoop(&Exit);
    /* Close CAN board */
    canClose(CANOpenShellOD_Data);
init_fail:
    TimerCleanup();
    return 0;
}

